Lab. 08 - SLAM Wizyjny (ORB-SLAM3) cz. 2

1. Cel zajęć

Celem zajęć jest uruchomienie lokalizacji 3D z wykorzystaniem systemu ORB-SLAM3 na danych z eksperymentów przeprowadzonych na terenie Politechniki Poznańskiej i ocena jakości działania systemu.

2. Przygotowanie środowiska

Na dzisiejszych zajęciach będzie wykorzystywany obraz z zajęć poprzednich, jedyną różnicą jest wykorzystywana paczka ORB_SLAM3_ROS2. Aktualizację obrazu można wykonać na dwa sposoby: - utworzyć obraz na nowo (należy pobrać zaktualizowany Dockerfile), - lub wywołać poniższe polecenia:

cd /arm_ws/src
rm -r orbslam3_ros2
git clone -b humble https://github.com/kamilmlodzikowski/ORB_SLAM3_ROS2 orbslam3_ros2
cd /arm_ws/src/orbslam3_ros2/vocabulary 
tar xvzf ORBvoc.txt.tar.gz
cd /arm_ws
rm -r build log install
source /opt/ros/humble/setup.bash
colcon build --symlink-install

Wersja 1: Obraz (większy rozmiar)

UWAGA: Wersja z obrazem może nie działać na części komputerów z uwagi na brak kompatybilności budowanych bibliotek. Sugerowana jest opcja samodzielnego zbudowania obrazu z wykorzystaniem Dockerfile’a!

Przed przystąpieniem do pracy należy przygotować środowisko: pobrać obraz dockera i utworzyć na jego podstawie kontener.

Obraz konieczny do wykonywania dzisiejszych zajęć opiera się o osrf/ros:humble-desktop-full, ale zawiera paczki, które są niezbędne do prawidłowego wykonania zadań. Obraz można pobrać z tego linku.

Pobrany obraz należy wczytać, korzystając z polecenia, podając odpowiednią ścieżkę:

docker load < path/to/file.tar.gz

Następnie można przejść do tworzenia kontenera korzystając ze skryptów przygotowanych dla osób korzystających wyłącznie z CPU lub wyposażonych w GPU Nvidia. Można pobrać odpowiedni skrypt korzystając np. z wget.

Domyślnie kontener nosi nazwę ARM_07. UWAGA! Skrypty po uruchomieniu usuwają kontener o takiej nazwie przed utworzeniem nowego.

Wersja 2: Dockerfile

Plik Dockerfile jest dostępny tutaj. Można go pobrać poleceniem:

wget https://raw.githubusercontent.com/kamilmlodzikowski/LabARM/main/Lab07-ORB-SLAM/Dockerfile

Skrypt budujący jest dostępny tutaj. Można go pobrać poleceniem:

wget https://raw.githubusercontent.com/kamilmlodzikowski/LabARM/main/Lab07-ORB-SLAM/arm_07_build.sh

Następnie można przejść do tworzenia kontenera korzystając ze skryptów przygotowanych dla osób korzystających wyłącznie z CPU lub wyposażonych w GPU Nvidia. Można pobrać odpowiedni skrypt korzystając np. z wget.

Domyślnie kontener nosi nazwę ARM_07. UWAGA! Skrypty po uruchomieniu usuwają kontener o takiej nazwie przed utworzeniem nowego.

Korzystanie z kontenera

Po każdym ponownym uruchomieniu komputera (oraz w przypadku problemów z wyświetlaniem aplikacji okienkowych w dockerze), proszę pamiętać o wywoływaniu:

xhost +local:root

Nowy terminal można dołączyć do kontenera korzystając z polecenia:

docker exec -it ARM_07 bash

ROS_DOMAIN_ID

W przypadku pracy na komputerze w laboratorium może okazać się konieczne ustawienie ROS_DOMAIN_ID. Domyślnie, ROS2 rozgłasza wszystko innym komputerom w sieci z tym samym ROS_DOMAIN_ID. Wartość tej zmiennej można ustawić korzystając z:

export ROS_DOMAIN_ID = <wybrana_wartość>

Proszę wybrać losową wartość.

Wygodne może być dodanie tego exportu do bashrc:

echo 'export ROS_DOMAIN_ID=<wybrana_wartość>' >> ~/.bashrc 

3. Uruchomienie ORB-SLAM3 na danych z PP

Przed przystąpieniem do tego kroku konieczne będą dane z DGPS, które powstały w ramach poprzednich zajęć. Jeżeli ktoś z jakiegoś powodu nie otrzymał własnego wyniku, proszę od kogoś pożyczyć plik dgps.txt. Oczywiście proszę tego pliku nie przesyłać jako rozwiązanie zadania domowego z poprzednich zajęć.

Przygotowanie kalibracji układu stereowizyjnego

Każde uruchomienie systemu na własnej konfiguracji kamer wymaga podania parametrów użytego układu kamer. W naszym przypadku chcemy tę konfigurację umieścić w pliku PP.yaml, który znajduje się pod ścieżką /arm_ws/src/orbslam3_ros2/config/stereo. Wartości parametrów tym pliku są aktualne dla innego układu stereowizyjnego, w ramach zajęć konieczna bedzie modyfikacja tych parametrów: - Camera.width - 720 - Camera.height - 540 - LEFT.height - 540 - LEFT.width - 720 - RIGHT.height - 540 - RIGHT.width - 720

Ponadto, aby poznać niektóre parametry konieczna będzie kalibracja układu. W tym przypadku zostało to już zrealizowane (wykorzystując pakiet kalibr). Uzyskano następujący wynik:

cam0:
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [-0.23248459123344795, 0.08191912671589296, 7.839314739418238e-05, 4.2075874672940684e-05]
  distortion_model: radtan
  intrinsics: [589.7363083173951, 590.0729541236071, 369.91183932674056, 298.34374442536097]
  resolution: [720, 540]
  rostopic: /pylon_stereo_node/left/image_raw
cam1:
  T_cn_cnm1:
  - [0.9997283077554007, 0.02320592542857902, 0.0021899081072863703, -0.32185797263546406]
  - [-0.0232254786906617, 0.9996863295720175, 0.00937121157638949, 0.0074867236444067856]
  - [-0.001971753560855992, -0.00941952715496201, 0.9999536912757833, -0.002949869596374046]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [-0.2303444874136257, 0.07876654742388302, 9.186197190170094e-05,
    -0.0005160611286370294]
  distortion_model: radtan
  intrinsics: [589.5069022640636, 589.958834706832, 379.29038618376575, 283.91319737264314]
  resolution: [720, 540]
  rostopic: /pylon_stereo_node/right/image_raw

Część tych parametrów można przepisać bezpośrednio do pliku PP.yaml (Camera.fx, Camera.fy, Camera.cx, Camera.cy, macierze D i K). Natomiast wartości macierzy R oraz P, a także Camera.bf można poznać dopiero po wykorzystaniu narzędzia stereoRectify z biblioteki OpenCV. W tym celu można skorzystać z przygotowanego skryptu. Znajduje się on w folderze scripts paczki orbslam3_ros2. Należy uzupełnić ten skrypt odpowiednimi wartościami.

Wartość Camera.bf znajdą Państwo jako wartość bezwzględną z wartości w macierzy P2 w prawym, górnym narożniku.

Uwaga! Należy zwrócić uwagę czy wynik kalibracji to układ prawej kamery w układzie lewej kamery czy też lewej w układzie prawej.

PUT Car

Po poprawnej kalibracji możliwe jest uruchomienie ORB-SLAM3 na danych z PP.

Baga można uruchomić poleceniem:

ros2 bag play -r 0.25 bags/put_car -p --remap /pylon_stereo_node/left/image_raw:=/camera/left /pylon_stereo_node/right/image_raw:=/camera/right

Uwaga! System ORB-SLAM3 jest dość wymagający obliczeniowo, więc przypominam o parametrze -r przy rosbag play, który umożliwia zwolnienie odtwarzania danych z rosbaga.

a ORB-SLAM3:

ros2 run orbslam3 stereo /arm_ws/src/orbslam3_ros2/vocabulary/ORBvoc.txt /arm_ws/src/orbslam3_ros2/config/stereo/PP.yaml true

Sugeruję użyć opcji Follow Camera, aby nawigowanie podczas tworzonej trajektorii było łatwiejsze.

Po zakończeniu działania systemu należy zapisać trajektorię klikając przycisk Stop w oknie ORB-SLAM3: Map Viewer.

4. Ewaluacja ORB-SLAM3

Ostatnim krokiem jest ewaluacja uzyskanego wyniku. Do ewaluacji wykorzystamy narzędzie evaluate_ate.py wraz z dodatkowym skryptem associate.py, które znajdą Państwo w katalogu /arm_ws/src/orbslam3_ros2/scripts. Korzystamy z niego następująco: shell cd /arm_ws/src/orbslam3_ros2/scripts/ python3 evaluate_ate.py DGPS_PATH ORBSLAM_PATH --verbose --plot trajs.png gdzie w miejsce DGPS_PATH podajemy ścieżkę do pliku dgps.txt, a w miejsce ORBSLAM_PATH ścieżkę do trajektorii zwróconej przez ORB-SLAM3.

Uzyskany wynik proszę zapisać. Narzędzie wylicza najlepsze nałożenie dwóch podanych trajektorii (brak konieczności znania transformacji pomiędzy układem DGPS i układem kamery) generując wykres porównujący obie trajektorie oraz wyliczający błąd.

Nałożone trajektorie można podejrzeć poleceniem:

apt install viewnior
viewnior trajs.png

5. Zadanie do samodzielnej realizacji

Państwa zadaniem jest umieszczenie na platformie e-kursy: - pliku PP.yaml (z rozszerzeniem .txt) - zdjęcia wygenerowanego narzędziem evaluate_ate.py - uzyskane wyniki liczbowe z evaluate_ate.py. System nie jest deterministyczny, więc wyniki będą prawdopodobnie różne dla każdego z Państwa.

_{Autorzy: Michał Nowicki, Kamil Młodzikowski}